module Positioning
  class Triangulation < Positioning::Base
    class Estimation
      # geom: estimated geometry
      # dist_vector: [rho_0, rho_1, ..., rho_i], difference of the distance
      #   from the ap to the current position calculated by rssi and estimation
      attr_accessor :geom, :dist_vector, :design_matrix
      # diff_vector: difference between the previous estimation and
      #   the new estimation
      attr_accessor :adaptability, :v, :s, :diff_vector

      def initialize(geom)
        @geom = geom
      end
    end

    class EstimationList
      attr_accessor :options, :estimations

      def initialize(options = {})
        @estimations = []
        @options = options
      end

      def init_geom(wifi_logs)
        # first estimation result is generated by WeightedAverage
        @estimations << Estimation.new(Positioning::WeightedAverage.new(:wifi_logs => wifi_logs).estimate_by_geom)
      end

      def init_geom_symmetric(estimation_list)
        first = estimation_list.estimations.first
        last = estimation_list.estimations.last
        # FIXME
        # the difference of longitude and latitude can not be used for
        # calculating "correct" symmetric point
        diff_lon = last.geom.lon - first.geom.lon
        diff_lat = last.geom.lat - first.geom.lat
        diff_z = last.geom.z - first.geom.z
        @estimations << Estimation.new(Point.from_lon_lat_z(first.geom.lon - diff_lon, first.geom.lat - diff_lat, first.geom.z - diff_z, Base::SRID))
      end

      def add_estimation
        estimated_geom = Base.point_in_dist(
          :start => geom,
          # to calculate brng replace x and y in atan2
          # e.g. Math.atan2(x, y)
          :rad_brng => Math.atan2(diff_vector[0], diff_vector[1]),
          :d => Math.sqrt(diff_vector.subvector(2).inner_product(diff_vector.subvector(2)))
        )
        estimated_geom.z += diff_vector[2]
        @estimations << Estimation.new(estimated_geom)
      end

      def geom
        @estimations.last.geom
      end

      def geom=(geom)
        @estimations.last.geom=(geom)
      end
      
      def dist_vector
        @estimations.last.dist_vector
      end

      def dist_vector=(dist_vector)
        @estimations.last.dist_vector=(dist_vector)
      end

      def design_matrix
        @estimations.last.design_matrix
      end

      def design_matrix=(design_matrix)
        @estimations.last.design_matrix=(design_matrix)
      end

      def adaptability
        @estimations.last.adaptability
      end

      def adaptability=(adaptability)
        @estimations.last.adaptability=(adaptability)
      end

      def diff_vector
        @estimations.last.diff_vector
      end

      def diff_vector=(diff_vector)
        @estimations.last.diff_vector=(diff_vector)
      end

      def s
        @estimations.last.s
      end

      def s=(s)
        @estimations.last.s=(s)
      end

      def v
        @estimations.last.v
      end

      def v=(v)
        @estimations.last.v=(v)
      end

      def converged?
        p @estimations[-1].geom
        p @estimations[-2].geom
        Base.haversine_distance(@estimations[-1].geom, @estimations[-2].geom) < @options[:tolerance_threthold]
      end

      def sort_s
        initial_s = @estimations[0].s
        inv_s = [1 / initial_s[0], 1 / initial_s[1], 1 / initial_s[2]]
        [inv_s.max, inv_s.min]
      end

      def s_ratio
        inv_s_max, inv_s_min = sort_s
        inv_s_max / inv_s_min
      end
      
      def need_second_stage?(threthold)
        s_ratio > threthold
      end
      
      def best_estimation
        @estimations.sort do |a, b|
          a.adaptability <=> b.adaptability
        end.first
      end
    end

    class << self # class methods
    end # class methods

    attr_reader :first_stage, :second_stage

    def initialize(options = {})
      super(options)
      @options = {
        :tolerance_threthold => 3.0,
        :second_stage_threthold => 3.0,
        :max_iteration => 10,
      }.merge(options)
    end

    def estimate_by_map
      x, y, z = Positioning::WeightedAverage.estimate_by_map(options)
    end

    # calculate 'adaptability' of the estimated geometry
    # adaptability = avg(difference of the distance from the observed
    # wifi_access_point (wifi_log) to the current position (rssi) and distance
    # from the observed wifi_access_point to the estimated geometry)
    # / the distance from the observed wifi_acces_point to the current
    # position  (rssi)
    # ->this calculation result pays much importance on errors occurred
    #   at observation points near from the AP.
    #
    def adaptability(geom)
      adaptability = 0.0
      @wifi_logs.each do |wifi_log|
        dist_by_rssi = Base.dist_value(wifi_log.signal)
        dist_to_estimated = Base.haversine_distance(wifi_log.wifi_access_point.manual_location.geom, geom)
        adaptability += (dist_to_estimated - dist_by_rssi).abs / dist_by_rssi
      end
      (adaptability / @wifi_logs.size)
    end


    # initialize observation equation for the specified estimated geometry
    def init_observation_equation(stage)
      # seed array of the vector rho_i (i = 0..n)
      dist_array = []
      # seed array of the matrix [alpha_i, beta_i, gamma_i] (i = 0..n)
      design_array = []
      @wifi_logs.each do |wifi_log|
        geom = wifi_log.wifi_access_point.manual_location.geom
        # calculate the distance from the observed WifiAccessPoint
        # to the estimated geom (MovementLog)
        dist_to_estimated = Base.haversine_distance(geom, stage.geom)
        # calculate the distance from the WifiAccessPoint
        # to the specified MovementLog by RSSI
        dist_by_rssi = Base.dist_value(wifi_log.signal)
        # rho_i
        dist_array << (dist_by_rssi - dist_to_estimated)
        # [alpha_i, beta_i, gamma_i]
        sign_lon = (-(geom.lon - stage.geom.lon) >= 0) ? 1 : -1
        sign_lat = (-(geom.lat - stage.geom.lat) >= 0) ? 1 : -1
        design_array << [sign_lon * Base.haversine_distance_x(geom, stage.geom) / dist_by_rssi,
                        sign_lat * Base.haversine_distance_y(geom, stage.geom) / dist_by_rssi,
                        - (geom.z - stage.geom.z)]
      end
      stage.dist_vector = GSL::Vector.alloc(dist_array)
      stage.design_matrix = GSL::Matrix.alloc(*design_array)
      stage.adaptability = adaptability(stage.geom)
    end

    def calc_least_square(stage)
      u, v, s = stage.design_matrix.SV_decomp
      stage.v = v
      stage.s = s
      stage.diff_vector = GSL::Vector.alloc(0, 0, 0)
      stage.s.size.times do |i|
        stage.diff_vector += u.col(i).trans.inner_product(stage.dist_vector) / stage.s[i] * stage.v.col(i).trans
      end
      stage.diff_vector
    end

    def estimation_loop(stage)
      max_iteration = @options[:max_iteration]
      max_iteration.times do |i|
        # generate observation equation
        init_observation_equation(stage)
        calc_least_square(stage)
        stage.add_estimation
        if stage.converged?
          break
        end
      end
      init_observation_equation(stage)
    end

    # estimate a location from wifi_logs
    # Options are:
    # * <tt>:wifi_logs</tt> - Specifies an array of WifiLog observed at current location
    #
    # Examples:
    #   Refer Positioning::WeightedAverage
    def estimate_by_geom
      # initialize EstimationList
      @first_stage = EstimationList.new(@options)
      # estimate initial location
      @first_stage.init_geom(@wifi_logs)
      estimation_loop(@first_stage)
      best_estimation = @first_stage.best_estimation
      if @first_stage.need_second_stage?(@options[:second_stage_threthold])
        @second_stage = EstimationList.new(@options)
        @second_stage.init_geom_symmetric(@first_stage)
        estimation_loop(@second_stage)
        candidate_estimation = @second_stage.best_estimation
        if best_estimation.adaptability > candidate_estimation.adaptability
          best_estimation = candidate_estimation
        end
      end
      best_estimation.geom
    end

    def estimate_ap_location_by_geom
    end
  end
end
